#include <surface\catmullRomSpline.h>

vec3f
catmullRomSpline::interpolate(float t, const vec3f& p1, const vec3f& p2, const vec3f& p3, const vec3f& p4)
{
  float t2 = t * t;
  float t3 = t2 * t;

  float b1 = .5f * (  -t3 + 2*t2 - t);
  float b2 = .5f * ( 3*t3 - 5*t2 + 2);
  float b3 = .5f * (-3*t3 + 4*t2 + t);
  float b4 = .5f * (   t3 -   t2    );

  return (p1*b1 + p2*b2 + p3*b3 + p4*b4); 
}

void 
catmullRomSpline::addSplinePoint(const vec3f& point) {
  points.push_back(point);
  delta_t = (float) 1 / (float) points.size();
}

vec3f 
catmullRomSpline::getInterpolatedSplinePoint(float t) {
    
  // Find out in which interval we are on the spline
  int p = (int)(t / delta_t);
  // Compute local control point indices
  #define BOUNDS(pp) { if (pp < 0) pp = 0; else if (pp >= (int)points.size()-1) pp = points.size() - 1; }
    
  int p0 = p - 1;     BOUNDS(p0);
  int p1 = p;         BOUNDS(p1);
  int p2 = p + 1;     BOUNDS(p2);
  int p3 = p + 2;     BOUNDS(p3);
  
  // Relative (local) time 
	float lt = (t - delta_t*(float)p) / delta_t;
  return catmullRomSpline::interpolate(lt, points[p0], points[p1], points[p2], points[p3]);
}

int 
catmullRomSpline::getNumPoints() {
	return points.size();
}

vec3f&
catmullRomSpline::getPoint(int n) {
	return points[n];
}